﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Engine.Physics
{
    public class OBB : CollisionShape
    {
        public Vector2[] Corners { get; private set; }
        public Vector2[] LocalAxis { get; private set; }
        private float[] Center;

        private List<Vector2> allCornersA = new List<Vector2>(4);
        private List<Vector2> allCornersB = new List<Vector2>(4);

        public Vector2 Position
        {
            get { return new Vector2(Center[0], Center[1]); }
            set
            {
                MoveTo(value);
            }
        }

        public override float Width
        {
            get
            {
                return Corners[3].X - Corners[0].X;
            }
        }

        public override float Height
        {
            get
            {
                return Corners[1].Y - Corners[0].Y;
            }
        }

        public OBB(Vector2 _center, float _width, float _height, float _rotation)
        {
            Reset(_center, _width, _height, _rotation);
        }

        private Vector2 GetMaxPointInternal(Vector2 _dir, out int _index)
        {
            float max = Vector2.Dot(Corners[0], _dir);
            Vector2 maxPoint = Corners[0];
            _index = 0;
            //Project remaining 3
            for (int j = 1; j < 4; j++)
            {
                float val = Vector2.Dot(Corners[j], _dir);
                //Update min/max projection on the axis
                if (val > max)
                {
                    max = val;
                    maxPoint = Corners[j];
                    _index = j;
                }
            }
            return maxPoint;
        }

        public override Vector2 GetMaxPoint(Vector2 _dir)
        {
            int index;
            return GetMaxPointInternal(_dir, out index);
        }

        public override void GetBestEdge(out Vector2 _max, out Vector2 _e1, out Vector2 _e2, Vector2 _n)
        {
            int index;
            _max = GetMaxPointInternal(_n, out index);

            Vector2 prev = Corners[index - 1 < 0 ? 3 : index - 1];
            Vector2 next = Corners[(index + 1) % 4];

            Vector2 l = _max - next;
            Vector2 r = _max - prev;

            if (Vector2.Dot(r, _n) <= Vector2.Dot(l, _n))
            {
                _e1 = prev;
                _e2 = _max;
            }
            else
            {
                _e1 = _max;
                _e2 = next;
            }
        } 

        public void Reset(Vector2 _center, float _width, float _height, float _rotation)
        {
            Vector2 xDimension = new Vector2((float)Math.Cos(_rotation), (float)Math.Sin(_rotation));
            Vector2 yDimension = new Vector2((float)-Math.Sin(_rotation), (float)Math.Cos(_rotation));

            xDimension *= _width / 2;
            yDimension *= _height / 2;

            Corners = new Vector2[4];
            Corners[0] = _center - xDimension - yDimension; //Top Left
            Corners[1] = _center - xDimension + yDimension; //Bottom Left
            Corners[2] = _center + xDimension + yDimension; //Bottom Right
            Corners[3] = _center + xDimension - yDimension; //Top Right

            Center = new float[2];
            Center[0] = _center.X;
            Center[1] = _center.Y;

            UpdateAxis();
        }

        /// <summary>
        /// Intersection test using Seperating Axis Theorem
        /// </summary>
        public bool Intersects(OBB _other, ref CollisionInfo _collInfo, Vector2 _relativeVelocity)
        {
            _collInfo.collding = true;
            _collInfo.willCollide = true;
            Vector2 shortestAxis = Vector2.Zero;
            float shortestProjection = float.PositiveInfinity;

            Vector2[] axes = new Vector2[4];
            axes[0] = LocalAxis[0];
            axes[1] = LocalAxis[1];
            axes[2] = _other.LocalAxis[0];
            axes[3] = _other.LocalAxis[1];

            allCornersA.Clear();
            allCornersA.Capacity = 4;
            allCornersA.Add(Corners[0]);
            allCornersA.Add(Corners[1]);
            allCornersA.Add(Corners[2]);
            allCornersA.Add(Corners[3]);

            allCornersB.Clear();
            allCornersB.Capacity = 4;
            allCornersB.Add(_other.Corners[0]);
            allCornersB.Add(_other.Corners[1]);
            allCornersB.Add(_other.Corners[2]);
            allCornersB.Add(_other.Corners[3]);
            //this-other test
            for (int i = 0; i < 4; i++)
            {
                /////////////////////////////////////////////////////////////////////
                //Project on the others objects coreners on this objects axis
                float aMax = 0;
                float aMin = 0;
                float[] cornerValuesA = new float[Corners.Length];
                ProjectToAxis(Corners, axes[i], ref aMin, ref aMax, ref cornerValuesA);

                /////////////////////////////////////////////////////////////////////
                //Project own corners on this axis
                float bMax = 0;
                float bMin = 0;

                float[] cornerValuesB = new float[_other.Corners.Length];
                ProjectToAxis(_other.Corners, axes[i], ref bMin, ref bMax, ref cornerValuesB);

                /////////////////////////////////////////////////////////////////////

                float gapWidth = GapWidth(aMin, aMax, bMin, bMax);
                //if the mins/max' don't overlap, get out
                if (gapWidth > 0)
                {
                    _collInfo.collding = false;
                    return false;
                }

                #region continuose
                /*float projectedVelocity = Vector2.Dot(axes[i], _relativeVelocity);

                if( projectedVelocity > 0)
                {
                    if(aMax < bMin)
                    {
                        _collInfo.willCollide = false;
                        return false;
                    }
                    else if ((aMin < bMin && bMin < aMax) || (bMin < aMin && aMin < bMax))
                    {
                        tMin = Math.Min(tMin, (aMax - bMin) / projectedVelocity);
                        tMax = Math.Max(tMax, 0);
                    }
                    else
                    {
                        tMin = Math.Min(tMin, (aMax - bMin) / projectedVelocity);
                        tMax = Math.Max(tMax, (aMin - bMax) / projectedVelocity);
                    }
                }
                else if (projectedVelocity < 0)
                {
                    if (bMax < aMin)
                    {
                        _collInfo.willCollide = false;
                        return false;
                    }
                    else if ((bMin < aMin && aMin < bMax) || (aMin < bMin && bMin < aMax))
                    {
                        tMin = Math.Min(tMin, (bMax - aMin) / projectedVelocity);
                        tMax = Math.Max(tMax, 0);
                    }
                    else
                    {
                        tMin = Math.Min(tMin, (bMax - aMin) / projectedVelocity);
                        tMax = Math.Max(tMax, (bMin - aMax) / projectedVelocity);
                    }
                }
                else if (projectedVelocity == 0)
                {
                    if (aMin < bMax && bMin < aMax)
                    {
                        tMin = Math.Min(tMin, 0);
                        tMax = Math.Max(tMax, 0);
                    }
                    else
                    {
                        _collInfo.willCollide = false;
                        return false;
                    }
                }
                //Update this
                if (projectedVelocity < 0)
                    aMin += projectedVelocity;
                else
                    aMax += projectedVelocity;

                gapWidth = GapWidth(aMin, aMax, bMin, bMax);
                if (gapWidth > 0)
                {
                    _collInfo.willCollide = false;
                }

                if (!_collInfo.willCollide && !_collInfo.collding)
                    break;*/
                #endregion

                if (aMin < bMin)
                {
                    for(int k = 0; k < cornerValuesA.Length; k++)
                    {
                        if (cornerValuesA[k] < bMin)
                            allCornersA.Remove(Corners[k]);

                        if (cornerValuesA[k] > bMax)
                            allCornersA.Remove(Corners[k]);
                    }

                    for (int k = 0; k < cornerValuesB.Length; k++)
                    {
                        if (cornerValuesB[k] > aMax)
                            allCornersB.Remove(_other.Corners[k]);
                    }
                }
                else
                {
                    for (int k = 0; k < cornerValuesB.Length; k++)
                    {
                        if (cornerValuesB[k] < aMin)
                            allCornersB.Remove(_other.Corners[k]);

                        if (cornerValuesB[k] > aMax)
                            allCornersB.Remove(_other.Corners[k]);
                    }

                    for (int k = 0; k < cornerValuesA.Length; k++)
                    {
                        if (cornerValuesA[k] > bMax)
                            allCornersA.Remove(Corners[k]);
                    }
                }

                gapWidth = Math.Abs(gapWidth);

                if (gapWidth < shortestProjection)
                {
                    shortestProjection = gapWidth;
                    shortestAxis = axes[i];

                    Vector2 distance = Position - _other.Position;
                    if (Vector2.Dot(distance, shortestAxis) < 0) 
                        shortestAxis = -shortestAxis;
                }

            }

            

            if (_collInfo.willCollide)
            {
                //set projection
                _collInfo.Normal = shortestAxis;
                _collInfo.MTV = shortestAxis * shortestProjection;

                foreach (Vector2 contactA in allCornersA)
                    _collInfo.ContactPoints.Add(contactA + _collInfo.MTV);

                foreach (Vector2 contactB in allCornersB)
                    _collInfo.ContactPoints.Add(contactB - _collInfo.MTV);
            }

            //if got to this point there is a collision
            return _collInfo.willCollide && _collInfo.collding;
        }

        private void ProjectToAxis(Vector2[] _corners, Vector2 _axis, ref float _min, ref float _max, ref float[] _cornerValues)
        {
            /////////////////////////////////////////////////////////////////////
            //Project on the others objects coreners on this objects axis
            //Project first corner
            float t1 = Vector2.Dot(_corners[0], _axis);

            _min = t1;
            _max = t1;
            _cornerValues[0] = t1;

            //Project remaining 3
            for (int j = 1; j < 4; j++)
            {
                t1 = Vector2.Dot(_corners[j], _axis);
                _cornerValues[j] = t1;
                //Update min/max projection on the axis
                if (t1 < _min)
                {     
                    _min = t1;
                }
                else if (t1 > _max)
                {
                    _max = t1;
                }
            }
        }

        private float GapWidth(float _t1Min, float _t1Max, float _t2Min, float _t2Max)
        {
            if (_t1Min < _t2Min)
                return _t2Min - _t1Max;
            else
                return _t1Min - _t2Max;
        }

        public void Offset(Vector2 _offset)
        {
            Center[0] += _offset.X;
            Center[1] += _offset.Y;
            for (int i = 0; i < 4; i++)
            {
                Corners[i] += _offset;
            }
            UpdateAxis();
        }

        private void UpdateAxis()
        {
            LocalAxis = new Vector2[2];
            LocalAxis[0] = Corners[3] - Corners[0];
            LocalAxis[1] = Corners[1] - Corners[0];

            LocalAxis[0].Normalize(); // /= LocalAxis[0].LengthSquared();
            LocalAxis[1].Normalize(); // /= LocalAxis[1].LengthSquared();
        }

        private void MoveTo(Vector2 _position)
        {
            Vector2 translation = _position - Position;

            Offset(translation);
        }
    }
}
